/*   The MIT License
 *   
 *   Carina Engine
 *   Copyright (c) 2009 2010 2011 2012 Zdravko Velinov
 *   
 *   Permission is hereby granted, free of charge, to any person obtaining a copy
 *   of this software and associated documentation files (the "Software"), to deal
 *   in the Software without restriction, including without limitation the rights
 *   to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 *   copies of the Software, and to permit persons to whom the Software is
 *   furnished to do so, subject to the following conditions:
 *
 *   The above copyright notice and this permission notice shall be included in
 *   all copies or substantial portions of the Software.
 *
 *   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 *   IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 *   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 *   AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 *   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 *   OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 *   THE SOFTWARE.
 */

#include "carina/common/global.hh"
#include "carina/collision-detection.hh"
#include "carina/common/types.hh"

#include <climits>
#include <stack>
#include <cstring>

namespace Carina
{
struct MinSumStack
{
    size_t      parent_index;
    size_t      begin_index;
    size_t      end_index;
    Vector3     mean;
    AABB        box;
    MinSumStack(size_t parent, size_t begin, size_t end, const Vector3& _mean, const AABB& _box)
        :   parent_index(parent),
            begin_index(begin),
            end_index(end),
            mean(_mean),
            box(_box) {}
};

AABBHierarchy::AABBHierarchy()
{
}

AABBHierarchy::AABBHierarchy(const AABBHierarchy& h)
    :   m_Tree(h.m_Tree),
        m_Indices(h.m_Indices),
        m_Data(h.m_Data)
{
    for(size_t i = 0; i < m_Tree.size(); ++i)
        if(m_Tree[i].is_leaf && m_Tree[i].triangles)
            m_Tree[i].triangles = CE_ALLOCATE(Triangles)(*h.m_Tree[i].triangles);
}

AABBHierarchy& AABBHierarchy::operator=(const AABBHierarchy& h)
{
    for(size_t i = 0; i < m_Tree.size(); ++i)
        if(m_Tree[i].is_leaf)
            CE_DEALLOCATE(m_Tree[i].triangles);
    m_Tree = h.m_Tree;
    m_Indices = h.m_Indices;
    m_Data = h.m_Data;
    for(size_t i = 0; i < m_Tree.size(); ++i)
        if(m_Tree[i].is_leaf && m_Tree[i].triangles)
            m_Tree[i].triangles = CE_ALLOCATE(Triangles)(*h.m_Tree[i].triangles);
    return *this;
}

AABBHierarchy::~AABBHierarchy()
{
    for(size_t i = 0; i < m_Tree.size(); ++i)
        if(m_Tree[i].is_leaf)
            CE_DEALLOCATE(m_Tree[i].triangles);
}

AABBNode& AABBHierarchy::getChild(size_t idx)
{
    return m_Tree[idx];
}

bool AABBHierarchy::buildMinSum(VertexData data, size_t data_size, const uint32* indices, size_t indices_size)
{
    typedef DynamicArray<uint32> IndiceArray;
    std::stack<MinSumStack> stack;
    MinSumStack cur(0, 0, indices_size, Vector3(0.0f, 0.0f, 0.0f), AABB());

    float               volume,
                        vtmp;
    uint32*             p_ind;
    size_t              ind_count,
                        split_index[3],
                        box_index,
                        c0,
                        c1,
                        cur_index = 0;
    AABB                boxes[3][2],
                        cbox[2];
    IndiceArray         split_indices[3];
    Vector3             bmean[3][2];

    if(indices_size % 3 != 0 || !data.data || !indices_size || !data_size || !indices)
        return false;
    
    clear();

    m_Data.assign(data.vec_data, data_size);

    m_Tree.push_back(AABBNode());
    m_Tree[cur_index].is_leaf = true;

    AABB& root = m_Tree[0].box;

    for(size_t i = 0; i < indices_size; ++i)
    {
        const Vector3& vec = data.vec_data[indices[i]];
        root.addPoint(vec);

        cur.mean += vec;
    }
    cur.mean /= (float)indices_size;

    m_Indices.assign(indices, indices_size);

    for(;;)
    {
        memset(bmean, 0, sizeof(bmean));

        p_ind = m_Indices.get() + cur.begin_index;
        ind_count = cur.end_index - cur.begin_index;
        split_index[0] = ind_count;
        split_index[1] = ind_count;
        split_index[2] = ind_count;

        split_indices[0].assign(p_ind, ind_count);
        split_indices[1].assign(p_ind, ind_count);
        split_indices[2].assign(p_ind, ind_count);

        for(size_t j = 0; j < 3; ++j)
        {
            boxes[j][0]._resetMax();
            boxes[j][1]._resetMax();
            bmean[j][0].set(0.0f, 0.0f, 0.0f);
            bmean[j][1].set(0.0f, 0.0f, 0.0f);
            for(size_t i = 0; i < split_index[j];)
            {
                const Vector3   &v0 = data.vec_data[split_indices[j][i]],
                                &v1 = data.vec_data[split_indices[j][i+1]],
                                &v2 = data.vec_data[split_indices[j][i+2]];
                bool    b0 = v0[j] >= cur.mean[j],
                        b1 = v1[j] >= cur.mean[j],
                        b2 = v2[j] >= cur.mean[j];
                if(b0 && b1 && b2)
                    box_index = 1;
                else if(!(b0 || b1 || b2))
                    box_index = 0;
                else
                {
                    cbox[0]._resetMax();
                    cbox[1]._resetMax();
                    cbox[(size_t)b0].addPoint(v0);
                    cbox[(size_t)b1].addPoint(v1);
                    cbox[(size_t)b2].addPoint(v2);
                    box_index = (size_t)(cbox[0].getVolume() <= cbox[1].getVolume());
                }
                
                boxes[j][box_index].addPoint(v0);
                boxes[j][box_index].addPoint(v1);
                boxes[j][box_index].addPoint(v2);
                bmean[j][box_index] += v0;
                bmean[j][box_index] += v1;
                bmean[j][box_index] += v2;
                if(box_index)
                {
                    split_index[j] -= 3;
                    std::swap(split_indices[j][i],   split_indices[j][split_index[j]]);
                    std::swap(split_indices[j][i+1], split_indices[j][split_index[j]+1]);
                    std::swap(split_indices[j][i+2], split_indices[j][split_index[j]+2]);
                }
                else
                    i += 3;
            }
        }
        split_index[0] += cur.begin_index;
        split_index[1] += cur.begin_index;
        split_index[2] += cur.begin_index;
        box_index = INT_MAX;
        if(split_index[0] != cur.end_index && split_index[0] != cur.begin_index)
            volume = boxes[0][0].getVolume() + boxes[0][1].getVolume(), box_index = 0;
        else
            volume = FLT_MAX;
        if(split_index[1] != cur.end_index && split_index[1] != cur.begin_index)
        {
            vtmp = boxes[1][0].getVolume() + boxes[1][1].getVolume();
            if(vtmp < volume)
                volume = vtmp, box_index = 1;
        }
        if(split_index[2] != cur.end_index && split_index[2] != cur.begin_index)
        {
            vtmp = boxes[2][0].getVolume() + boxes[2][1].getVolume();
            if(vtmp < volume)
                volume = vtmp, box_index = 2;
        }
        if(box_index < 3)
        {
            std::copy(split_indices[box_index].begin(), split_indices[box_index].end(), m_Indices.begin() + cur.begin_index);
            c0 = split_index[box_index] - cur.begin_index;
            c1 = cur.end_index - split_index[box_index];
            stack.push(MinSumStack(cur_index, split_index[box_index], cur.end_index, bmean[1][box_index] / (float)c1, boxes[box_index][1]));
            cur.end_index = split_index[box_index]; 
            cur.mean = bmean[0][box_index] / (float)c0;
            cur.parent_index = cur_index;

            m_Tree[cur_index++].is_leaf = false;
            m_Tree.push_back(AABBNode());
            m_Tree[cur_index].is_leaf = true;
            m_Tree[cur_index].box = boxes[box_index][0];
        }
        else
        {
            m_Tree[cur_index].triangles = CE_ALLOCATE(Triangles)(m_Indices.get() + cur.begin_index, cur.end_index - cur.begin_index);
            if(!stack.empty())
            {
                cur = stack.top();
                stack.pop();
                m_Tree[cur.parent_index].second_index = ++cur_index;
                m_Tree.push_back(AABBNode());
                m_Tree[cur_index].is_leaf = true;
                m_Tree[cur_index].box = cur.box;
            }
            else
                break;
        }
    }
    return true;
}

struct TestStack
{
    size_t a_node, b_node;
    TestStack(size_t a_idx, size_t b_idx)
        :   a_node(a_idx),
            b_node(b_idx) {}
};

bool AABBHierarchy::test(const AABBHierarchy& h) const
{
    const DynamicArray<AABBNode>& tree = h.m_Tree;
    std::stack<TestStack> stack;
    for(size_t i = 0, j = 0;;)
    {
        const AABB  &a = m_Tree[i].box,
                    &b = tree[j].box;
        if(a.testCollision(b))
        {
            if(m_Tree[i].is_leaf)
                return false;
            else if(!stack.empty())
            {
                i = stack.top().a_node;
                j = stack.top().b_node;
                stack.pop();
            }
            else
                return false;
        }
        if(m_Tree[i].is_leaf)
        {
            if(tree[j].is_leaf)
                if(testGeometry(m_Tree[i].triangles, tree[i].triangles))
                    break;
                else if(!stack.empty())
                {
                    i = stack.top().a_node;
                    j = stack.top().b_node;
                    stack.pop();
                }
                else
                    return false;
            else
                stack.push(TestStack(i, tree[j++].second_index));
        }
        else
            stack.push(TestStack(m_Tree[i++].second_index, j));
    }

    return true;
}

bool AABBHierarchy::testIntersection(const Ray& ray) const
{
    std::stack<size_t> stack;

    for(size_t curidx = 0;;)
    {
        const AABBNode& cur_node = m_Tree[curidx];
        if(cur_node.box.testIntersection(ray))
        {
            if(cur_node.is_leaf)
            {
                // TODO: test intersection with geometry
                break;
            }
        }
        else
        {
            if(stack.empty())
                return false;
            else
            {
                curidx = stack.top();
                stack.pop();
            }
        }
    }

    return true;
}

bool AABBHierarchy::testIntersection(const Segment& seg) const
{
    std::stack<size_t> stack;

    for(size_t curidx = 0;;)
    {
        const AABBNode& cur_node = m_Tree[curidx];
        if(cur_node.box.testIntersection(seg))
        {
            if(cur_node.is_leaf)
            {
                // TODO: test intersection with geometry
                break;
            }
        }
        else
        {
            if(stack.empty())
                return false;
            else
            {
                curidx = stack.top();
                stack.pop();
            }
        }
    }

    return true;
}

void AABBHierarchy::_update(const AABBHierarchy& tree, const Matrix4& matrix)
{
    for(size_t i = 0; i < m_Tree.size(); ++i)
    {
        m_Tree[i].box.min = matrix * tree.m_Tree[i].box.min;
        m_Tree[i].box.max = matrix * tree.m_Tree[i].box.max;
    }
    for(size_t i = 0; i < tree.m_Data.size(); ++i)
        m_Data[i] = matrix * tree.m_Data[i];
}

void AABBHierarchy::clear()
{
    for(size_t i = 0; i < m_Tree.size(); ++i)
        if(m_Tree[i].is_leaf)
            CE_DEALLOCATE(m_Tree[i].triangles);
    m_Indices.clear();
    m_Data.clear();
    m_Tree.clear();
}

bool AABBHierarchy::testGeometry(Triangles* lhs, Triangles* rhs) const
{
    return true;
}

bool AABB::testIntersection(const Ray& ray) const
{
    float   t0,
            t1,
            tmin = 0.0f,
            tmax = FLT_MAX;

    if(ray.direction.x() == 0.0f)
    {
        if(ray.begin.x() > max.x() || ray.begin.x() < min.x())
            return false;
    }
    else
    {
        t0 = (min.x() - ray.begin.x()) / ray.direction.x();
        t1 = (max.x() - ray.begin.x()) / ray.direction.x();
        if(t0 > t1)
            std::swap(t0, t1);
        if(t0 > tmin)
            tmin = t0;
        if(t1 < tmax)
            tmax = t1;
        if(t0 > t1)
            return false;
    }

    if(ray.direction.y() == 0.0f)
    {
        if(ray.begin.y() > max.y() || ray.begin.y() < min.y())
            return false;
    }
    else
    {
        t0 = (min.y() - ray.begin.y()) / ray.direction.y();
        t1 = (max.y() - ray.begin.y()) / ray.direction.y();
        if(t0 > t1)
            std::swap(t0, t1);
        if(t0 > tmin)
            tmin = t0;
        if(t1 < tmax)
            tmax = t1;
        if(t0 > t1)
            return false;
    }

    if(ray.direction.z() == 0.0f)
    {
        if(ray.begin.z() > max.z() || ray.begin.z() < min.z())
            return false;
    }
    else
    {
        t0 = (min.z() - ray.begin.z()) / ray.direction.z();
        t1 = (max.z() - ray.begin.z()) / ray.direction.z();
        if(t0 > t1)
            std::swap(t0, t1);
        if(t0 > tmin)
            tmin = t0;
        if(t1 < tmax)
            tmax = t1;
        if(t0 > t1)
            return false;
    }

    return true;
}

bool AABB::testIntersection(const Segment& seg) const
{
    Vector3 c = getCenter(),
            r = c - min,
            m = seg.getMidPoint(),
            d = m - seg.begin;
    m = m - c;
    Vector3 ad = v3abs(d),
            am = v3abs(m);
    if(am.x() > r.x() + ad.x() && am.y() > r.y() + ad.y() && am.z() > r.z() + ad.z() &&
       fabs(m.y()*d.z() - m.z()*d.y()) > r.y()*ad.z() + r.z()*ad.y() &&
       fabs(m.z()*d.x() - m.x()*d.z()) > r.x()*ad.z() + r.z()*ad.x() &&
       fabs(m.x()*d.y() - m.y()*d.x()) > r.x()*ad.y() + r.y()*ad.x())
        return false;
    return true;
}

void AABB::addPoint(const Vector3& vec)
{
    if(vec.x() < min.x())
        min.x() = vec.x();
    if(vec.x() > max.x())
        max.x() = vec.x();

    if(vec.y() < min.y())
        min.y() = vec.y();
    if(vec.y() > max.y())
        max.y() = vec.y();

    if(vec.z() < min.z())
        min.z() = vec.z();
    if(vec.z() > max.z())
        max.z() = vec.z();
}

float AABB::getVolume() const
{
    Vector3 box = max - min;
    return box.x()*box.y()*box.z();
}

float AABB::getSurface() const
{
    Vector3 box = max - min;
    return 2.0f*(box.x()*box.y() + box.x()*box.z() + box.y()*box.z());
}
}
